#include "arm_control/arm_action_type8.hpp"
#include <chrono>
#include <thread>
#include <algorithm>

//C区右臂抓取
// 右臂xuan11运动到-0.7853，然后运动到C_right_pose，然后根据检测数据进行抓取

namespace arm_control
{

ArmActionType8::ArmActionType8(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 创建订阅者
  right_sub_ = node_->create_subscription<fruit_detector::msg::DetectionArray>(
    "/detect_right", 10,
    std::bind(&ArmActionType8::right_callback, this, std::placeholders::_1));
  
  // 创建爪子发布器
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
  
  // 初始化MoveGroupInterface for 右臂
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  // 创建 VoicePlanner action 客户端
  voice_planner_client_ = rclcpp_action::create_client<VoicePlanner>(node_, "voice_planner");
  
  RCLCPP_INFO(logger_, "动作类型8初始化完成");
  RCLCPP_INFO(logger_, "订阅话题: /detect_right");
  RCLCPP_INFO(logger_, "爪子发布器已创建: /zhuazi_right");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化，Planning Group: arm_right");
  RCLCPP_INFO(logger_, "arm_right规划参考坐标系: %s", move_group_right_->getPlanningFrame().c_str());
  RCLCPP_INFO(logger_, "VoicePlanner客户端已创建");
}

void ArmActionType8::right_callback(const fruit_detector::msg::DetectionArray::SharedPtr msg)
{
  right_data_.clear();
  for (const auto& fruit : msg->fruits) {
    FruitData8 data;
    data.type = fruit.type;
    data.offset_x = fruit.offset_x;
    data.offset_y = fruit.offset_y;
    data.isgood = fruit.isgood;
    right_data_.push_back(data);
  }
  RCLCPP_DEBUG(logger_, "收到右侧检测数据，共 %zu 个水果", right_data_.size());
}

FruitData8 ArmActionType8::select_best_fruit(
  const std::vector<int32_t>& type_array,
  const std::vector<FruitData8>& data,
  bool& found)
{
  found = false;
  FruitData8 best_fruit;
  float min_offset = std::numeric_limits<float>::max();
  
  // 遍历所有数据
  for (const auto& fruit : data) {
    // 检查是否在类型数组中
    auto it = std::find(type_array.begin(), type_array.end(), fruit.type);
    if (it != type_array.end()) {
      // 计算offset
      float offset = fruit.get_offset();
      // 如果offset更小，则更新最佳水果
      if (offset < min_offset) {
        min_offset = offset;
        best_fruit = fruit;
        found = true;
      }
    }
  }
  
  return best_fruit;
}

bool ArmActionType8::move_down(
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group,
  double distance,
  const std::string& arm_name,
  double offset_x,
  double offset_y)
{
  RCLCPP_INFO(logger_, "[%s] 开始移动: 向下=%.3f米, offset_x=%.2f, offset_y=%.2f", 
              arm_name.c_str(), distance, offset_x, offset_y);
  
  // ========== xyz同时规划 ==========
  RCLCPP_INFO(logger_, "[%s] === xyz同时规划 ===", arm_name.c_str());
  
  // 更新当前状态
  move_group->setStartStateToCurrentState();
  RCLCPP_INFO(logger_, "[%s] 已更新当前机械臂状态", arm_name.c_str());
  
  // 获取当前末端位姿
  geometry_msgs::msg::PoseStamped current_pose = move_group->getCurrentPose();
  RCLCPP_INFO(logger_, "[%s] 当前末端位置: x=%.3f, y=%.3f, z=%.3f", 
              arm_name.c_str(),
              current_pose.pose.position.x,
              current_pose.pose.position.y,
              current_pose.pose.position.z);
  
  // 创建路径点（同时调整x、y、z）
  std::vector<geometry_msgs::msg::Pose> waypoints;
  geometry_msgs::msg::Pose target_pose = current_pose.pose;
  
  // 右臂：x为-offset_x * 0.0008，y为offset_y * 0.0008
  target_pose.position.x -= offset_x * 0.0008;
  target_pose.position.y += offset_y * 0.0008;
  // z轴向下移动
  target_pose.position.z -= distance;
  
  waypoints.push_back(target_pose);
  
  RCLCPP_INFO(logger_, "[%s] 目标位置: x=%.3f, y=%.3f, z=%.3f (xyz同时移动)", 
              arm_name.c_str(),
              target_pose.position.x,
              target_pose.position.y,
              target_pose.position.z);
  
  // 计算笛卡尔路径（xyz同时规划）
  moveit_msgs::msg::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;  // 禁用跳跃检测
  const double eef_step = 0.01;       // 1cm 步长
  
  double fraction = move_group->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  
  RCLCPP_INFO(logger_, "[%s] 笛卡尔路径规划完成度: %.2f%%", arm_name.c_str(), fraction * 100.0);
  
  if (fraction < 0.80) {  // 至少完成80%的路径
    RCLCPP_ERROR(logger_, "[%s] 笛卡尔路径规划失败，完成度不足", arm_name.c_str());
    return false;
  }
  
  // 执行轨迹（xyz同时移动）
  RCLCPP_INFO(logger_, "[%s] 开始执行xyz同时运动...", arm_name.c_str());
  auto execute_result = move_group->execute(trajectory);
  
  if (execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "[%s] 运动执行失败！", arm_name.c_str());
    return false;
  }
  
  RCLCPP_INFO(logger_, "[%s] xyz同时运动执行成功！", arm_name.c_str());
  return true;
}

void ArmActionType8::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型8 - 右臂xuan11运动到-0.7853，然后运动到C_right_pose，然后根据检测数据抓取");
  
  const auto goal = goal_handle->get_goal();
  
  // 获取右侧类型数组
  const auto& right_types = goal->right_type;
  
  RCLCPP_INFO(logger_, "右侧类型数组大小: %zu", right_types.size());
  
  // ========== 第一步：将xuan11运动到-0.7853 ==========
  
  // 更新右臂状态
  feedback->progress_percentage = 5.0;
  feedback->current_status = "更新右臂状态...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态...");
  move_group_right_->setStartStateToCurrentState();
  
  // 将xuan11运动到-0.7853
  feedback->progress_percentage = 10.0;
  feedback->current_status = "右臂xuan11移动到-0.7853...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "将右臂xuan11设置为-0.7853");
  
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前状态并修改xuan11关节
  moveit::core::RobotStatePtr current_state_right = move_group_right_->getCurrentState(10.0);
  if (!current_state_right) {
    RCLCPP_ERROR(logger_, "无法获取右臂当前机器人状态");
    result->success = false;
    result->message = "动作类型8失败 - 无法获取右臂当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取所有关节的当前值
  std::vector<double> joint_group_positions_right;
  const moveit::core::JointModelGroup* joint_model_group_right = 
    current_state_right->getJointModelGroup("arm_right");
  current_state_right->copyJointGroupPositions(joint_model_group_right, joint_group_positions_right);
  
  // 只修改xuan11的值
  const std::vector<std::string>& joint_names_right = joint_model_group_right->getVariableNames();
  bool found_joint = false;
  for (size_t i = 0; i < joint_names_right.size(); i++) {
    if (joint_names_right[i] == "xuan11") {
      joint_group_positions_right[i] = -0.7853;
      RCLCPP_INFO(logger_, "找到xuan11关节，设置为-0.7853");
      found_joint = true;
      break;
    }
  }
  
  if (!found_joint) {
    RCLCPP_ERROR(logger_, "未找到xuan11关节");
    result->success = false;
    result->message = "动作类型8失败 - 未找到xuan11关节";
    goal_handle->abort(result);
    return;
  }
  
  move_group_right_->setJointValueTarget(joint_group_positions_right);
  
  moveit::planning_interface::MoveGroupInterface::Plan xuan11_plan;
  bool xuan11_success = (move_group_right_->plan(xuan11_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!xuan11_success) {
    result->success = false;
    result->message = "动作类型8失败 - 右臂xuan11路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂xuan11路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂xuan11路径规划成功");
  
  auto xuan11_execute_result = move_group_right_->execute(xuan11_plan);
  if (xuan11_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型8失败 - 右臂xuan11运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂xuan11运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂xuan11已到达-0.7853");
  
  // 等待机械臂稳定
  RCLCPP_INFO(logger_, "等待0.5秒让机械臂稳定...");
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  
  // 更新状态
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（xuan11运动后）");
  move_group_right_->setStartStateToCurrentState();
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型8已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型8已取消");
    return;
  }
  
  // ========== 第二步：右臂运动到C_right_pose ==========
  
  feedback->progress_percentage = 30.0;
  feedback->current_status = "更新机械臂状态并移动到C_right_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（准备移动到C_right_pose）");
  move_group_right_->setStartStateToCurrentState();
  
  RCLCPP_INFO(logger_, "设置右臂目标姿态为C_right_pose");
  
  move_group_right_->setStartStateToCurrentState();
  move_group_right_->setNamedTarget("C_right_pose");
  
  moveit::planning_interface::MoveGroupInterface::Plan right_plan;
  bool right_success = (move_group_right_->plan(right_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!right_success) {
    result->success = false;
    result->message = "动作类型8失败 - 右臂到C_right_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到C_right_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂到C_right_pose路径规划成功");
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型8已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型8已取消");
    return;
  }
  
  feedback->progress_percentage = 40.0;
  feedback->current_status = "右臂正在移动到C_right_pose...";
  goal_handle->publish_feedback(feedback);
  
  auto right_execute_result = move_group_right_->execute(right_plan);
  if (right_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型8失败 - 右臂到C_right_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到C_right_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂已到达C_right_pose");
  
  // 等待机械臂稳定
  RCLCPP_INFO(logger_, "等待0.5秒让机械臂稳定...");
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  
  // 更新状态
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（C_right_pose到达后）");
  move_group_right_->setStartStateToCurrentState();
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型8已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型8已取消");
    return;
  }
  
  // ========== 第三步：等待检测数据并执行抓取 ==========
  
  // 等待一段时间确保已收到检测数据
  feedback->progress_percentage = 50.0;
  feedback->current_status = "等待检测数据...";
  goal_handle->publish_feedback(feedback);
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  
  std::string result_message = "动作类型8执行成功";
  bool grab_success = false;
  
  if (!right_types.empty()) {
    feedback->progress_percentage = 55.0;
    feedback->current_status = "分析右侧检测数据...";
    goal_handle->publish_feedback(feedback);
    
    // ========== 播报offset最小的水果（无论是否为目标类型） ==========
    if (!right_data_.empty()) {
      RCLCPP_INFO(logger_, "检测到 %zu 个水果，准备播报offset最小的水果...", right_data_.size());
      
      // 找到offset最小的水果
      float min_offset = std::numeric_limits<float>::max();
      FruitData8 closest_fruit;
      bool found_closest = false;
      
      for (const auto& fruit : right_data_) {
        float offset = fruit.get_offset();
        if (offset < min_offset) {
          min_offset = offset;
          closest_fruit = fruit;
          found_closest = true;
        }
      }
      
      if (found_closest) {
        feedback->progress_percentage = 56.0;
        feedback->current_status = "播报offset最小的水果...";
        goal_handle->publish_feedback(feedback);
        
        // 根据水果类型确定num1参数
        // 模型3类别ID: 0-成熟辣椒, 1-未熟辣椒, 2-成熟南瓜, 3-未熟南瓜, 4-成熟洋葱, 5-未熟洋葱, 6-成熟西红柿, 7-未熟西红柿
        // voiceplanner num1: 1-成熟洋葱, 2-未熟洋葱, 3-成熟南瓜, 4-未熟南瓜, 5-成熟西红柿, 6-未熟西红柿, 7-成熟辣椒, 8-未熟辣椒
        int32_t voice_num1;
        switch(closest_fruit.type) {
          case 0: voice_num1 = 7; break;  // 成熟辣椒
          case 1: voice_num1 = 8; break;  // 未熟辣椒
          case 2: voice_num1 = 3; break;  // 成熟南瓜
          case 3: voice_num1 = 4; break;  // 未熟南瓜
          case 4: voice_num1 = 1; break;  // 成熟洋葱
          case 5: voice_num1 = 2; break;  // 未熟洋葱
          case 6: voice_num1 = 5; break;  // 成熟西红柿
          case 7: voice_num1 = 6; break;  // 未熟西红柿
          default: voice_num1 = 0; break; // 未知类型
        }
        RCLCPP_INFO(logger_, "播报offset最小的水果: type=%d, voice_num1=%d, offset=%.2f", 
                    closest_fruit.type, voice_num1, closest_fruit.get_offset());
        
        // 等待voice_planner服务可用
        if (!voice_planner_client_->wait_for_action_server(std::chrono::seconds(2))) {
          RCLCPP_WARN(logger_, "VoicePlanner action服务器不可用，跳过语音播报");
        } else {
          // 创建voice_planner目标
          auto voice_goal = VoicePlanner::Goal();
          voice_goal.type = 6;
          voice_goal.data.push_back(voice_num1);
          
          RCLCPP_INFO(logger_, "发送voice_planner请求: type=6, data[0]=%d", voice_num1);
          
          // 异步发送goal，不等待结果（避免阻塞）
          auto send_goal_options = rclcpp_action::Client<VoicePlanner>::SendGoalOptions();
          send_goal_options.result_callback = 
            [this](const rclcpp_action::ClientGoalHandle<VoicePlanner>::WrappedResult & result) {
              if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
                RCLCPP_INFO(logger_, "语音播报完成: %s", result.result->message.c_str());
              } else {
                RCLCPP_WARN(logger_, "语音播报失败");
              }
            };
          
          voice_planner_client_->async_send_goal(voice_goal, send_goal_options);
          RCLCPP_INFO(logger_, "已发送语音播报请求，继续执行后续动作");
          
          // 等待1秒让语音播报开始
          std::this_thread::sleep_for(std::chrono::seconds(1));
        }
      }
    } else {
      RCLCPP_INFO(logger_, "未检测到任何水果");
    }
    
    // ========== 从目标类型中选择要抓取的水果 ==========
    bool right_found = false;
    FruitData8 right_fruit = select_best_fruit(right_types, right_data_, right_found);
    
    if (right_found) {
      RCLCPP_INFO(logger_, "从目标类型中找到匹配水果，准备抓取: 类型=%d, offset=%.2f", 
                  right_fruit.type, right_fruit.get_offset());
      
      // 执行右侧抓取动作
      feedback->progress_percentage = 60.0;
      feedback->current_status = "准备抓取右侧水果...";
      goal_handle->publish_feedback(feedback);
      
      RCLCPP_INFO(logger_, "目标水果(右侧): 类型=%d, offset_x=%.2f, offset_y=%.2f, isgood=%d", 
                  right_fruit.type, right_fruit.offset_x, 
                  right_fruit.offset_y, right_fruit.isgood);
      
      // 先更新机械臂状态
      RCLCPP_INFO(logger_, "更新右侧机械臂起始状态为当前状态");
      move_group_right_->setStartStateToCurrentState();
      
      feedback->progress_percentage = 65.0;
      feedback->current_status = "正在执行右侧机械臂向下运动...";
      goal_handle->publish_feedback(feedback);
      
      if (goal_handle->is_canceling()) {
        result->success = false;
        result->message = "动作类型8已取消(抓取中)";
        goal_handle->canceled(result);
        RCLCPP_INFO(logger_, "动作类型8已取消(抓取中)");
        return;
      }
      
      bool right_move_success = move_down(move_group_right_, 0.38, "右臂", 
                                          right_fruit.offset_x, right_fruit.offset_y);
      
      if (right_move_success) {
        RCLCPP_INFO(logger_, "右侧水果抓取完成");
        
        // 发布右爪子数据0（关闭）
        auto right_msg = std_msgs::msg::Int32();
        right_msg.data = 0;
        zhuazi_right_publisher_->publish(right_msg);
        RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 0 (关闭)");
        
        // 等待3秒让爪子动作完成
        RCLCPP_INFO(logger_, "等待3秒让爪子动作完成...");
        std::this_thread::sleep_for(std::chrono::seconds(3));
        
        result_message += " - 右侧抓取成功";
        result_message += " - 右侧(类型=" + std::to_string(right_fruit.type) + 
                         ", offset=" + std::to_string(right_fruit.get_offset()) + ")";
        grab_success = true;
        
        feedback->progress_percentage = 70.0;
        feedback->current_status = "右侧抓取完成";
        goal_handle->publish_feedback(feedback);
        
        // ========== 抓取后的操作：yuntai1移动到0.00 ==========
        feedback->progress_percentage = 72.0;
        feedback->current_status = "更新机械臂状态并将yuntai1移动到0.00...";
        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（抓取后）");
        move_group_right_->setStartStateToCurrentState();
        
        RCLCPP_INFO(logger_, "将右臂yuntai1设置为0.00");
        
        move_group_right_->setStartStateToCurrentState();
        
        // 获取当前状态并修改yuntai1关节
        moveit::core::RobotStatePtr current_state_yuntai = move_group_right_->getCurrentState(10.0);
        if (!current_state_yuntai) {
          RCLCPP_ERROR(logger_, "无法获取右臂当前机器人状态(yuntai1)");
          result->success = false;
          result->message = "动作类型8失败 - 无法获取右臂当前状态(yuntai1)";
          goal_handle->abort(result);
          return;
        }
        
        // 获取所有关节的当前值
        std::vector<double> joint_group_positions_yuntai;
        const moveit::core::JointModelGroup* joint_model_group_yuntai = 
          current_state_yuntai->getJointModelGroup("arm_right");
        current_state_yuntai->copyJointGroupPositions(joint_model_group_yuntai, joint_group_positions_yuntai);
        
        // 只修改yuntai1的值
        const std::vector<std::string>& joint_names_yuntai = joint_model_group_yuntai->getVariableNames();
        bool found_yuntai1 = false;
        for (size_t i = 0; i < joint_names_yuntai.size(); i++) {
          if (joint_names_yuntai[i] == "yuntai1") {
            joint_group_positions_yuntai[i] = 0.00;
            RCLCPP_INFO(logger_, "找到yuntai1关节，设置为0.00");
            found_yuntai1 = true;
            break;
          }
        }
        
        if (!found_yuntai1) {
          RCLCPP_WARN(logger_, "未找到yuntai1关节，跳过该步骤");
        } else {
          move_group_right_->setJointValueTarget(joint_group_positions_yuntai);
          
          moveit::planning_interface::MoveGroupInterface::Plan yuntai1_plan;
          bool yuntai1_success = (move_group_right_->plan(yuntai1_plan) == moveit::core::MoveItErrorCode::SUCCESS);
          
          if (!yuntai1_success) {
            RCLCPP_WARN(logger_, "右臂yuntai1路径规划失败，跳过该步骤");
          } else {
            RCLCPP_INFO(logger_, "右臂yuntai1路径规划成功");
            
            auto yuntai1_execute_result = move_group_right_->execute(yuntai1_plan);
            if (yuntai1_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
              RCLCPP_WARN(logger_, "右臂yuntai1运动执行失败，跳过该步骤");
            } else {
              RCLCPP_INFO(logger_, "右臂yuntai1已到达0.00");
              // 等待机械臂稳定
              RCLCPP_INFO(logger_, "等待0.5秒让机械臂稳定...");
              std::this_thread::sleep_for(std::chrono::milliseconds(500));
            }
          }
        }
        
        // 检查是否取消
        if (goal_handle->is_canceling()) {
          result->success = false;
          result->message = "动作类型8已取消";
          goal_handle->canceled(result);
          RCLCPP_INFO(logger_, "动作类型8已取消");
          return;
        }
        
        // ========== 移动到right_gain_pose ==========
        feedback->progress_percentage = 75.0;
        feedback->current_status = "更新机械臂状态并移动到right_gain_pose...";
        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（yuntai1运动后）");
        move_group_right_->setStartStateToCurrentState();
        
        RCLCPP_INFO(logger_, "设置右臂目标姿态为right_gain_pose");
        
        move_group_right_->setStartStateToCurrentState();
        move_group_right_->setNamedTarget("right_gain_pose");
        
        moveit::planning_interface::MoveGroupInterface::Plan gain_plan;
        bool gain_success = (move_group_right_->plan(gain_plan) == moveit::core::MoveItErrorCode::SUCCESS);
        
        if (!gain_success) {
          RCLCPP_WARN(logger_, "右臂到right_gain_pose路径规划失败，跳过该步骤");
        } else {
          RCLCPP_INFO(logger_, "右臂到right_gain_pose路径规划成功");
          
          feedback->progress_percentage = 80.0;
          feedback->current_status = "右臂正在移动到right_gain_pose...";
          goal_handle->publish_feedback(feedback);
          
          auto gain_execute_result = move_group_right_->execute(gain_plan);
          if (gain_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
            RCLCPP_WARN(logger_, "右臂到right_gain_pose运动执行失败，跳过该步骤");
          } else {
            RCLCPP_INFO(logger_, "右臂已到达right_gain_pose");
            // 等待机械臂稳定
            RCLCPP_INFO(logger_, "等待0.5秒让机械臂稳定...");
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
          }
        }
        
        // 检查是否取消
        if (goal_handle->is_canceling()) {
          result->success = false;
          result->message = "动作类型8已取消";
          goal_handle->canceled(result);
          RCLCPP_INFO(logger_, "动作类型8已取消");
          return;
        }
        
        // 更新状态确保位置同步
        RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（准备释放爪子）");
        move_group_right_->setStartStateToCurrentState();
        
        // ========== 释放爪子 ==========
        feedback->progress_percentage = 85.0;
        feedback->current_status = "释放爪子...";
        goal_handle->publish_feedback(feedback);
        
        auto release_msg = std_msgs::msg::Int32();
        release_msg.data = 120;
        zhuazi_right_publisher_->publish(release_msg);
        RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120 (释放)");
        
        // 等待2秒让爪子释放动作完成
        RCLCPP_INFO(logger_, "等待2秒让爪子释放动作完成...");
        std::this_thread::sleep_for(std::chrono::seconds(2));
        
        feedback->progress_percentage = 90.0;
        feedback->current_status = "释放完成";
        goal_handle->publish_feedback(feedback);
        
        // ========== 张开爪子 ==========
        feedback->progress_percentage = 92.0;
        feedback->current_status = "张开爪子...";
        goal_handle->publish_feedback(feedback);
        
        auto open_msg = std_msgs::msg::Int32();
        open_msg.data = 120;
        zhuazi_right_publisher_->publish(open_msg);
        RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120 (张开)");
        
        // 等待1秒让爪子张开动作完成
        RCLCPP_INFO(logger_, "等待1秒让爪子张开动作完成...");
        std::this_thread::sleep_for(std::chrono::seconds(1));
        
        feedback->progress_percentage = 93.0;
        feedback->current_status = "张开完成";
        goal_handle->publish_feedback(feedback);
        
        // 检查是否取消
        if (goal_handle->is_canceling()) {
          result->success = false;
          result->message = "动作类型8已取消";
          goal_handle->canceled(result);
          RCLCPP_INFO(logger_, "动作类型8已取消");
          return;
        }
        
        // ========== 将xuan11移动到-0.7853 ==========
        feedback->progress_percentage = 94.0;
        feedback->current_status = "更新机械臂状态并将xuan11移动到-0.7853...";
        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（准备移动xuan11）");
        move_group_right_->setStartStateToCurrentState();
        
        RCLCPP_INFO(logger_, "将右臂xuan11设置为-0.7853");
        
        move_group_right_->setStartStateToCurrentState();
        
        // 获取当前状态并修改xuan11关节
        moveit::core::RobotStatePtr current_state_xuan11 = move_group_right_->getCurrentState(10.0);
        if (!current_state_xuan11) {
          RCLCPP_ERROR(logger_, "无法获取右臂当前机器人状态(xuan11)");
          result->success = false;
          result->message = "动作类型8失败 - 无法获取右臂当前状态(xuan11)";
          goal_handle->abort(result);
          return;
        }
        
        // 获取所有关节的当前值
        std::vector<double> joint_group_positions_xuan11;
        const moveit::core::JointModelGroup* joint_model_group_xuan11 = 
          current_state_xuan11->getJointModelGroup("arm_right");
        current_state_xuan11->copyJointGroupPositions(joint_model_group_xuan11, joint_group_positions_xuan11);
        
        // 只修改xuan11的值
        const std::vector<std::string>& joint_names_xuan11 = joint_model_group_xuan11->getVariableNames();
        bool found_xuan11 = false;
        for (size_t i = 0; i < joint_names_xuan11.size(); i++) {
          if (joint_names_xuan11[i] == "xuan11") {
            joint_group_positions_xuan11[i] = -0.7853;
            RCLCPP_INFO(logger_, "找到xuan11关节，设置为-0.7853");
            found_xuan11 = true;
            break;
          }
        }
        
        if (!found_xuan11) {
          RCLCPP_WARN(logger_, "未找到xuan11关节，跳过该步骤");
        } else {
          move_group_right_->setJointValueTarget(joint_group_positions_xuan11);
          
          moveit::planning_interface::MoveGroupInterface::Plan xuan11_final_plan;
          bool xuan11_final_success = (move_group_right_->plan(xuan11_final_plan) == moveit::core::MoveItErrorCode::SUCCESS);
          
          if (!xuan11_final_success) {
            RCLCPP_WARN(logger_, "右臂xuan11路径规划失败，跳过该步骤");
          } else {
            RCLCPP_INFO(logger_, "右臂xuan11路径规划成功");
            
            auto xuan11_final_execute_result = move_group_right_->execute(xuan11_final_plan);
            if (xuan11_final_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
              RCLCPP_WARN(logger_, "右臂xuan11运动执行失败，跳过该步骤");
            } else {
              RCLCPP_INFO(logger_, "右臂xuan11已到达-0.7853");
              // 等待机械臂稳定
              RCLCPP_INFO(logger_, "等待0.5秒让机械臂稳定...");
              std::this_thread::sleep_for(std::chrono::milliseconds(500));
            }
          }
        }
        
        // 检查是否取消
        if (goal_handle->is_canceling()) {
          result->success = false;
          result->message = "动作类型8已取消";
          goal_handle->canceled(result);
          RCLCPP_INFO(logger_, "动作类型8已取消");
          return;
        }
        
        // ========== 归位到C_right_pose ==========
        feedback->progress_percentage = 96.0;
        feedback->current_status = "归位到C_right_pose...";
        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态（准备归位）");
        move_group_right_->setStartStateToCurrentState();
        
        RCLCPP_INFO(logger_, "设置右臂目标姿态为C_right_pose（归位）");
        move_group_right_->setStartStateToCurrentState();
        move_group_right_->setNamedTarget("C_right_pose");
        
        moveit::planning_interface::MoveGroupInterface::Plan return_plan;
        bool return_success = (move_group_right_->plan(return_plan) == moveit::core::MoveItErrorCode::SUCCESS);
        
        if (!return_success) {
          RCLCPP_WARN(logger_, "右臂归位到C_right_pose路径规划失败，跳过该步骤");
        } else {
          RCLCPP_INFO(logger_, "右臂归位到C_right_pose路径规划成功");
          
          feedback->progress_percentage = 98.0;
          feedback->current_status = "右臂正在归位到C_right_pose...";
          goal_handle->publish_feedback(feedback);
          
          auto return_execute_result = move_group_right_->execute(return_plan);
          if (return_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
            RCLCPP_WARN(logger_, "右臂归位到C_right_pose运动执行失败，跳过该步骤");
          } else {
            RCLCPP_INFO(logger_, "右臂已归位到C_right_pose");
            // 等待机械臂稳定
            RCLCPP_INFO(logger_, "等待0.5秒让机械臂稳定...");
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
          }
        }
        
      } else {
        RCLCPP_WARN(logger_, "右侧机械臂运动失败");
        result_message += " - 右侧运动失败";
      }
      
    } else {
      RCLCPP_WARN(logger_, "右侧未找到匹配的水果");
      result_message += " - 右侧未找到匹配水果";
    }
  } else {
    RCLCPP_INFO(logger_, "右侧类型数组为空，跳过抓取");
    result_message += " - 右侧类型数组为空";
    
    // 如果类型数组为空，也发布打开爪子的消息
    feedback->progress_percentage = 95.0;
    feedback->current_status = "发布爪子消息...";
    goal_handle->publish_feedback(feedback);
    
    auto zhuazi_msg = std_msgs::msg::Int32();
    zhuazi_msg.data = 120;
    zhuazi_right_publisher_->publish(zhuazi_msg);
    RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120");
    
    // 等待1秒让爪子消息生效
    RCLCPP_INFO(logger_, "等待1秒让爪子消息生效...");
    std::this_thread::sleep_for(std::chrono::seconds(1));
  }
  
  // 动作完成
  feedback->progress_percentage = 100.0;
  feedback->current_status = "动作类型8任务完成";
  goal_handle->publish_feedback(feedback);
  
  if (grab_success || right_types.empty()) {
    result->success = true;
    result->message = result_message;
    goal_handle->succeed(result);
    RCLCPP_INFO(logger_, "动作类型8执行成功");
  } else {
    result->success = false;
    result->message = "动作类型8执行失败 - 未成功执行抓取动作";
    goal_handle->abort(result);
    RCLCPP_WARN(logger_, "动作类型8执行失败 - 未成功执行抓取动作");
  }
}

} // namespace arm_control

